#include "tracker_manager.h"
#include "tracker.h"

#include <algorithm>
#include <cmath>
#include <iostream>
#include <set>

#include "../global.h"

namespace fusion_perception {

std::shared_ptr<Tracker> TrackerManager::getTrackerById(int64_t id)
{
    if (trackersUMap_.find(id) != trackersUMap_.end()) {
        return trackersUMap_[id];
    }

    return nullptr;
}

ObjectSptrVector TrackerManager::getFusedObjects(void)
{
    auto& global = Global::getInstance();
    ObjectSptrVector objectSptrVector;

    for (auto begin = trackersUMap_.begin(); begin != trackersUMap_.end();) {
        if (begin->second->getStatus() == -1) {
            begin++;
            continue;
        }

        if (begin->second->getStatus() > global.hz * global.predictS) {
            begin = trackersUMap_.erase(begin);
            continue;
        }

        objectSptrVector.emplace_back(begin->second->get());
        begin++;
    }

    return objectSptrVector;
}

void TrackerManager::fuse(int64_t microTs, const MultiAssociatedObject& multiAssociatedObject)
{
    std::set<int64_t> fusedIds;

    for (auto& i : multiAssociatedObject) {
        std::shared_ptr<Tracker> trackerSptr { nullptr };

        if (i.first == nullptr) {
            trackerSptr = std::make_shared<Tracker>(id_++);
            trackersUMap_.emplace(trackerSptr->getId(), trackerSptr);
        } else {
            trackerSptr = trackersUMap_[i.first.get()->id];
        }

        fusedIds.emplace(trackerSptr->getId());

        trackerSptr->fuse(microTs, i.second);

        std::cout << trackerSptr->getId() << ":[";
        for (auto& j : i.second) {
            std::cout << j->header.frame_id << "-" << j->id << ",";
        }
        std::cout << "]" << std::endl;
    }

    for (auto& i : trackersUMap_) {
        if (fusedIds.find(i.first) == fusedIds.end()) { // unfused
            // std::cout << i.second->getId() << ":[predict:" << i.second->getStatus();
            // std::cout << "]" << std::endl;
            i.second->fuse(microTs, {});
        }
    }
}

}